{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<style>/* div {\n",
       "    max-width: 900px !important;\n",
       "    text-align: justify;\n",
       "} */\n",
       "\n",
       "\n",
       "/* .inner_cell {\n",
       "    max-width: 900px !important;\n",
       "    text-align: justify;\n",
       "} */\n",
       "\n",
       ".cell {\n",
       "    /* max-width: 900px !important; */\n",
       "    /* line-height: 1.6em; */\n",
       "    padding-bottom: 15px;\n",
       "    text-align: justify;\n",
       "}\n",
       "\n",
       ".markup {\n",
       "    max-width: 900px !important;\n",
       "    text-align: justify;\n",
       "}\n",
       "\n",
       "img {\n",
       "    display: block;\n",
       "    margin-left: auto;\n",
       "    margin-right: auto;\n",
       "}\n",
       "\n",
       "/* αt */\n",
       "/* βt */\n",
       "/* γt */</style>"
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 1,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Apply custon style to notebook\n",
    "from IPython.core.display import HTML\n",
    "import pathlib\n",
    "styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n",
    "styles = open(styles_path, \"r\").read()\n",
    "HTML(f\"<style>{styles}</style>\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "_Note: This notebook must be run locally. Due to how the Swift Simulator operates, this notebook will not run on Google Colab_"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 5.0 Quadratic Programming for Motion Control\n",
    "\n",
    "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part II: Acceleration and Advanced Applications}}$\n",
    "\n",
    "$\\text{By Jesse Haviland and Peter Corke}$\n",
    "\n",
    "<br>\n",
    "\n",
    "The sections of the Tutorial paper related to this notebook are listed next to each contents entry.\n",
    "It is beneficial to read these sections of the paper before attempting the notebook Section.\n",
    "\n",
    "### Contents\n",
    "\n",
    "[5.1 Swift and Robot Setup](#swift)\n",
    "\n",
    "[5.2 Basic Quadratic Programming](#bqp)\n",
    "* Advanced Velocity Control\n",
    "  * Null-space Projection\n",
    "  * Quadratic Programming\n",
    "\n",
    "[5.3 Velocity Dampers](#vd)\n",
    "* Advanced Velocity Control\n",
    "  * Null-space Projection\n",
    "  * Quadratic Programming\n",
    "\n",
    "[5.4 Adding Slack](#slack)\n",
    "* Advanced Velocity Control\n",
    "  * Null-space Projection\n",
    "  * Quadratic Programming"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We will do the imports required for this notebook here\n",
    "\n",
    "# numpy provides import array and linear algebra utilities\n",
    "import numpy as np\n",
    "\n",
    "# the robotics toolbox provides robotics specific functionality\n",
    "import roboticstoolbox as rtb\n",
    "\n",
    "# spatial math provides objects for representing transformations\n",
    "import spatialmath as sm\n",
    "\n",
    "# swift is a lightweight browser-based simulator which comes eith the toolbox\n",
    "from swift import Swift\n",
    "\n",
    "# spatialgeometry is a utility package for dealing with geometric objects\n",
    "import spatialgeometry as sg\n",
    "\n",
    "# this package provides several solvers for solving quadratic programmes\n",
    "import qpsolvers as qp"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='swift'></a>\n",
    "\n",
    "### 5.1 Swift and Robot Setup\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this notebook we will be using the Robotics Toolbox for Python and Swift to simulate our motion controllers. Check out Part 1 Notebook 3 for an introduction to these packages."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52013/?53013\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x28dd8acd0>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "2"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Make the environment\n",
    "env = Swift()\n",
    "\n",
    "# Launch the simulator\n",
    "# The realtime flag will ask the simulator to simulate as close as\n",
    "# possible to realtime as apposed to as fast as possible\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "\n",
    "# Make a panda robot\n",
    "panda = rtb.models.Panda()\n",
    "\n",
    "# Set the joint coordinates to qr\n",
    "panda.q = panda.qr\n",
    "\n",
    "# We can then add our robot to the simulator envionment\n",
    "env.add(panda)\n",
    "\n",
    "# end-effector axes\n",
    "ee_axes = sg.Axes(0.1)\n",
    "\n",
    "# goal axes\n",
    "goal_axes = sg.Axes(0.1)\n",
    "\n",
    "# Add the axes to the environment\n",
    "env.add(ee_axes)\n",
    "env.add(goal_axes) "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Make a variable for the upper and lower limits of the robot\n",
    "qd_lb = -20.0 * np.ones(panda.n)\n",
    "qd_ub = 20.0 * np.ones(panda.n)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='bqp'></a>\n",
    "\n",
    "### 4.2 Basic Quadratic Programming\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this Notebook, we are going redefine our motion controllers as a quadratic programming (QP) optimisation problem rather than a matrix equation. In general, a constrained QP is formulated as\n",
    "\n",
    "\\begin{align*} \n",
    "    \\min_x \\quad f_o(\\bf{x}) &= \\frac{1}{2} \\bf{x}^\\top \\bf{Q} \\bf{x}+ \\bf{c}^\\top \\bf{x}, \\\\ \n",
    "    \\text{subject to} \\quad \\bf{A}_1 \\bf{x} &= \\bf{b}_1,  \\\\\n",
    "    \\bf{A}_2 \\bf{x} &\\leq \\bf{b}_2,  \\\\\n",
    "    \\bf{g} &\\leq  \\bf{x} \\leq \\bf{h}.  \n",
    "\\end{align*}\n",
    "\n",
    "where $f_o(\\bf{x})$ is the objective function which is subject to the following equality and inequality constraints, and $\\bf{h}$ and $\\bf{g}$ represent the upper and lower bounds of $\\bf{x}$. A quadratic program is strictly convex when the matrix $\\bf{Q}$ is positive definite. This framework allows us to solve the same problems as above, but with additional flexibility.\n",
    "\n",
    "We can rewrite RRMC (see Part 1 Notebook 3) as a QP\n",
    "\n",
    "\\begin{align*} \n",
    "\t\\min_{\\dot{\\bf{q}}} \\quad f_o(\\dot{\\bf{q}}) \n",
    "\t&= \n",
    "    \\frac{1}{2} \n",
    "    \\dot{\\bf{q}}^\\top \n",
    "    \\bf{1}_n \n",
    "    \\dot{\\bf{q}}, \\\\\n",
    "\t\\text{subject to} \\quad\n",
    "\t\\bf{J}(\\bf{q}) \\dot{\\bf{q}} &= \\bf{\\nu},\n",
    "\t \\\\\n",
    "\t\\dot{\\bf{q}}^- &\\leq \\dot{\\bf{q}} \\leq \\dot{\\bf{q}}^+  \n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{1}_n \\in \\mathbb{R}^{n \\times n}$ is an identity matrix, and we have imposed $\\dot{\\bf{q}}^-$ and $\\dot{\\bf{q}}^+$ as upper and lower joint-velocity limits. If the robot has more degrees-of-freedom than necessary to reach its entire task space, the QP will achieve the desired end-effector velocity with the minimum joint-velocity norm (the same result as the psuedoinverse solution).\n",
    "\n",
    "Lets test this out in Python"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52014/?53014\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x28de6fd10>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make a new environment and add objects\n",
    "env = Swift()\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "env.set_camera_pose([1.3, 0, 0.4], [0, 0, 0.3])\n",
    "env.add(panda)\n",
    "env.add(ee_axes)\n",
    "env.add(goal_axes)\n",
    "\n",
    "# Change the robot configuration to a ready position\n",
    "panda.q = [0.21, -0.03, 0.35, -1.90, -0.04, 1.96, 1.36]\n",
    "\n",
    "# A variable to specify when to break the loop\n",
    "arrived = False\n",
    "\n",
    "# Specify the gain for the p_servo method\n",
    "kt = 1.0\n",
    "kr = 1.3\n",
    "k = np.array([kt, kt, kt, kr, kr, kr])\n",
    "\n",
    "# Specify our timestep\n",
    "dt = 0.05\n",
    "\n",
    "Tep = (\n",
    "    panda.fkine(panda.q)\n",
    "    * sm.SE3.Tx(-0.1)\n",
    "    * sm.SE3.Ty(0.6)\n",
    "    * sm.SE3.Tz(0.4)\n",
    ")\n",
    "Tep = Tep.A\n",
    "\n",
    "# Set the goal axes to Tep\n",
    "goal_axes.T = Tep\n",
    "\n",
    "# The quadratic component of the objective function\n",
    "Q = np.eye(panda.n)\n",
    "\n",
    "# Linear component of objective function\n",
    "c = np.ones(panda.n)\n",
    "\n",
    "# Run the simulation until the robot arrives at the goal\n",
    "while not arrived:\n",
    "\n",
    "    # Work out the base frame manipulator Jacobian using the current robot configuration\n",
    "    J = panda.jacob0(panda.q)\n",
    "\n",
    "    # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n",
    "    Te = panda.fkine(panda.q).A\n",
    "\n",
    "    # Calculate the required end-effector velocity and whether the robot has arrived\n",
    "    ev, arrived = rtb.p_servo(Te, Tep, gain=k, threshold=0.001, method=\"angle-axis\")\n",
    "\n",
    "    ### Calculate each component of the quadratic programme\n",
    "    # The equality contraints\n",
    "    Aeq = J\n",
    "    beq = ev.reshape((6,))\n",
    "\n",
    "    # The inequality constraints\n",
    "    Ain = None\n",
    "    bin = None\n",
    "\n",
    "    # The lower and upper bounds on the joint velocity\n",
    "    lb = qd_lb\n",
    "    ub = qd_ub\n",
    "\n",
    "    # Solve for the joint velocities qd and apply to the robot\n",
    "    panda.qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=lb, ub=ub, solver='quadprog')\n",
    "\n",
    "    # Update the ee axes\n",
    "    ee_axes.T = Te\n",
    "\n",
    "    # Step the simulator by dt seconds\n",
    "    env.step(dt)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can rewrite Park's controller (see Part 2 Notebook 4) as\n",
    "\n",
    "\\begin{align*} \n",
    "    \\min_{\\dot{\\bf{q}}} \\quad f_o(\\dot{\\bf{q}}) \n",
    "    &= \n",
    "    \\frac{1}\n",
    "        {2} \n",
    "    \\dot{\\bf{q}}^\\top \\lambda_q \\bf{1}_n \\dot{\\bf{q}} - \\lambda_m \\ \\bf{J}_m^\\top(\\bf{q}) \\dot{\\bf{q}}, \\\\ \n",
    "    \\text{subject to} \\quad \\bf{J}(\\bf{q}) \\dot{\\bf{q}} &= \\bf{\\nu},\n",
    "\t \\\\\n",
    "\t\\dot{\\bf{q}}^- &\\leq \\dot{\\bf{q}} \\leq \\dot{\\bf{q}}^+  \n",
    "\\end{align*}\n",
    "\n",
    "where the manipulability Jacobian fits into the linear component of the objective function.\n",
    "\n",
    "Lets implement this below"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52015/?53015\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x291053d90>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make a new environment and add objects\n",
    "env = Swift()\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "env.set_camera_pose([1.3, 0, 0.4], [0, 0, 0.3])\n",
    "env.add(panda)\n",
    "env.add(ee_axes)\n",
    "env.add(goal_axes) \n",
    "\n",
    "# Change the robot configuration to a ready position\n",
    "panda.q = [0.21, -0.03, 0.35, -1.90, -0.04, 1.96, 1.36]\n",
    "\n",
    "# A variable to specify when to break the loop\n",
    "arrived = False\n",
    "\n",
    "# Specify the gain for the p_servo method\n",
    "kt = 1.0\n",
    "kr = 1.3\n",
    "k = np.array([kt, kt, kt, kr, kr, kr])\n",
    "\n",
    "# Specify our timestep\n",
    "dt = 0.05\n",
    "\n",
    "Tep = (\n",
    "    panda.fkine(panda.q)\n",
    "    * sm.SE3.Tx(-0.1)\n",
    "    * sm.SE3.Ty(0.6)\n",
    "    * sm.SE3.Tz(0.4)\n",
    ")\n",
    "Tep = Tep.A\n",
    "\n",
    "# Set the goal axes to Tep\n",
    "goal_axes.T = Tep\n",
    "\n",
    "# Set the gain on the manipulability maximisation\n",
    "λm = 10.0\n",
    "\n",
    "# Set the gain on the joint velocity norm minimisation\n",
    "λq = 1.0\n",
    "\n",
    "Q = λq * np.eye(panda.n)\n",
    "\n",
    "# Run the simulation until the robot arrives at the goal\n",
    "while not arrived:\n",
    "\n",
    "    # Work out the base frame manipulator Jacobian using the current robot configuration\n",
    "    J = panda.jacob0(panda.q)\n",
    "\n",
    "    # Calculate the manipulability Jacobian\n",
    "    Jm = panda.jacobm(panda.q, axes='rot')\n",
    "\n",
    "    # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n",
    "    Te = panda.fkine(panda.q).A\n",
    "\n",
    "    # Calculate the required end-effector velocity and whether the robot has arrived\n",
    "    ev, arrived = rtb.p_servo(Te, Tep, gain=k, threshold=0.001, method=\"angle-axis\")\n",
    "\n",
    "    ### Calculate each component of the quadratic programme\n",
    "    # The equality contraints\n",
    "    Aeq = J\n",
    "    beq = ev.reshape((6,))\n",
    "\n",
    "    # The inequality constraints\n",
    "    Ain = None\n",
    "    bin = None\n",
    "\n",
    "    # Linear component of objective function: the manipulability Jacobian\n",
    "    c = λm * -Jm.reshape((panda.n,))\n",
    "\n",
    "    # The lower and upper bounds on the joint velocity\n",
    "    lb = qd_lb\n",
    "    ub = qd_ub\n",
    "\n",
    "    # Solve for the joint velocities qd and apply to the robot\n",
    "    panda.qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=lb, ub=ub, solver='quadprog')\n",
    "\n",
    "    # Update the ee axes\n",
    "    ee_axes.T = Te\n",
    "\n",
    "    # Step the simulator by dt seconds\n",
    "    env.step(dt)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='vd'></a>\n",
    "\n",
    "### 5.3 Velocity Dampers\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can expand the above with velocity dampers to enable joint-position limit avoidance. Velocity dampers are used to constrain velocities and dampen an input velocity as some limit is approached. The velocity is only damped if it is within some influence distance to the limit. A joint velocity is constrained to prevent joint limits using a velocity damper as\n",
    "\n",
    "\\begin{align*} \n",
    "    \\dot{q} \\leq\n",
    "    \\eta\n",
    "    \\frac{\\rho - \\rho_s}\n",
    "        {\\rho_i - \\rho_s}\n",
    "    \\qquad \\text{if} \\ \\rho < \\rho_i\n",
    "\\end{align*}\n",
    "\n",
    "where $\\rho \\in \\mathbb{R}^+$ is the distance or angle to the nearest joint limit, $\\eta \\in \\mathbb{R}^+$ is a gain which adjusts the aggressiveness of the damper, $\\rho_i$ is the influence distance in which to activate the damper, and $\\rho_s$ is the stopping distance in which the distance $\\rho$ will never be able to reach or enter. We can stack velocity dampers to perform joint position limit avoidance for each joint within a robot, and incorporate it into our QP as an inequality constraint\n",
    "\n",
    "\\begin{align*} \n",
    "    \\bf{1}_n\n",
    "    \\dot{\\bf{q}} &\\leq\n",
    "    \\eta\n",
    "    \\begin{pmatrix}\n",
    "        \\frac{\\rho_0 - \\rho_s}\n",
    "            {\\rho_i - \\rho_s} \\\\\n",
    "        \\vdots \\\\\n",
    "        \\frac{\\rho_n - \\rho_s}\n",
    "            {\\rho_i - \\rho_s} \n",
    "    \\end{pmatrix}\n",
    "\\end{align*}\n",
    "\n",
    "where the identity $\\bf{1}_n$ is included to show how the equation fits into the general form $\\bf{A}\\bf{x} \\leq \\bf{b}$ of an inequality constraint.\n",
    "\n",
    "Lets make a Python method to construct these joint position limit velocity dampers"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "def joint_velocity_damper(robot, ps, pi, gain):\n",
    "    \"\"\"\n",
    "    Formulates an inequality contraint which, when optimised for will\n",
    "    make it impossible for the robot to run into joint limits. Requires\n",
    "    the joint limits of the robot to be specified.\n",
    "\n",
    "    ps: The minimum angle (in radians) in which the joint is\n",
    "        allowed to approach to its limit\n",
    "    pi: The influence angle (in radians) in which the velocity\n",
    "        damper becomes active\n",
    "    gain: The gain for the velocity damper\n",
    "\n",
    "    returns: Ain, Bin as the inequality contraints for an qp\n",
    "    \"\"\"\n",
    "\n",
    "    Ain = np.zeros((robot.n, robot.n))\n",
    "    Bin = np.zeros(robot.n)\n",
    "\n",
    "    for i in range(robot.n):\n",
    "        if robot.q[i] - robot.qlim[0, i] <= pi:\n",
    "            Bin[i] = -gain * (((robot.qlim[0, i] - robot.q[i]) + ps) / (pi - ps))\n",
    "            Ain[i, i] = -1\n",
    "        if robot.qlim[1, i] - robot.q[i] <= pi:\n",
    "            Bin[i] = gain * ((robot.qlim[1, i] - robot.q[i]) - ps) / (pi - ps)\n",
    "            Ain[i, i] = 1\n",
    "\n",
    "    return Ain, Bin"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='slack'></a>\n",
    "\n",
    "### 5.4 Adding Slack\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It is possible that the robot will fail to reach the goal when the constraints create local minima. In such a scenario, the error $\\bf{e}$ in the PBS scheme is no longer decreasing and the robot can no longer progress due to the constraints in the QP.\n",
    "\n",
    "The methods shown up to this point require a redundant robot, where the number of joints is greater than 6. In [8], extra redundancy was introduced to the QP by relaxing the equality constraint in the QP to allow for intentional deviation from the desired end-effector velocity $\\bf{\\nu}$. The slack has the additional benefit of providing the solver extra flexibility to meet constraints and avoid local minima. The augmented QP is defined as\n",
    "\n",
    "\\begin{align*} \n",
    "    \\min_x \\quad f_o(\\bf{x}) &= \\frac{1}{2} \\bf{x}^\\top \\mathcal{Q} \\bf{x}+ \\mathcal{C}^\\top \\bf{x}, \\\\ \n",
    "    \\text{subject to} \\quad \\mathcal{J} \\bf{x} &= \\bf{\\nu},  \\\\\n",
    "    \\mathcal{A} \\bf{x} &\\leq \\mathcal{B},  \\\\\n",
    "    \\bf{x}^- &\\leq \\bf{x} \\leq \\bf{x}^+ \n",
    "\\end{align*}\n",
    "\n",
    "with\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{x} &= \n",
    "    \\begin{pmatrix}\n",
    "        \\dot{\\bf{q}} \\\\ \\bf{\\delta}\n",
    "    \\end{pmatrix} \\in \\mathbb{R}^{(n+6)}  \\\\\n",
    "    \\mathcal{Q} &=\n",
    "    \\begin{pmatrix}\n",
    "        \\lambda_q \\bf{1}_{n} & \\mathbf{0}_{6 \\times 6} \\\\ \\mathbf{0}_{n \\times n} & \\lambda_\\delta \\bf{1}_{6}\n",
    "    \\end{pmatrix} \\in \\mathbb{R}^{(n+6) \\times (n+6)} \\\\\n",
    "    \\mathcal{J} &=\n",
    "    \\begin{pmatrix}\n",
    "        \\bf{J}(\\bf{q}) & \\bf{1}_{6}\n",
    "    \\end{pmatrix} \\in \\mathbb{R}^{6 \\times (n+6)} \\\\\n",
    "    \\mathcal{C} &= \n",
    "    \\begin{pmatrix}\n",
    "        \\bf{J}_m \\\\ \\bf{0}_{6 \\times 1}\n",
    "    \\end{pmatrix} \\in \\mathbb{R}^{(n + 6)} \\\\\n",
    "    \\mathcal{A} &= \n",
    "    \\begin{pmatrix}\n",
    "        \\bf{1}_{n \\times n + 6} \\\\\n",
    "    \\end{pmatrix} \\in \\mathbb{R}^{(l + n) \\times (n + 6)} \\\\\n",
    "    \\mathcal{B} &= \n",
    "    \\eta\n",
    "    \\begin{pmatrix}\n",
    "        \\frac{\\rho_0 - \\rho_s}\n",
    "                {\\rho_i - \\rho_s} \\\\\n",
    "        \\vdots \\\\\n",
    "        \\frac{\\rho_n - \\rho_s}\n",
    "                {\\rho_i - \\rho_s} \n",
    "    \\end{pmatrix} \\in \\mathbb{R}^{n} \\\\\n",
    "    \\bf{x}^{-, +} &= \n",
    "    \\begin{pmatrix}\n",
    "        \\dot{\\bf{q}}^{-, +} \\\\\n",
    "        \\bf{\\delta}^{-, +}\n",
    "    \\end{pmatrix} \\in \\mathbb{R}^{(n+6)},\n",
    "\\end{align*}\n",
    "\n",
    "where\n",
    "$\\bf{\\delta} \\in \\mathbb{R}^6$ is the slack vector,\n",
    "$\\lambda_\\delta \\in \\mathbb{R}^+$ is a gain term which adjusts the cost of the norm of the slack vector in the optimiser,\n",
    "$\\dot{\\bf{q}}^{-,+}$ are the minimum and maximum joint velocities, and \n",
    "$\\dot{\\bf{\\delta}}^{-,+}$ are the minimum and maximum slack velocities.\n",
    "Each of the gains can be adjusted dynamically. For example, in practice $\\lambda_\\delta$ is typically large when far from the goal, but reduces towards 0 as the goal approaches.\n",
    "\n",
    "The effect of this augmented optimisation problem is that the equality constraint is equivalent to\n",
    "\n",
    "\\begin{align*} \n",
    "    \\bf{\\nu}(t) - \\bf{\\delta}(t) = \\bf{J}(\\bf{q}) \\dot{\\bf{q}}(t)\n",
    "\\end{align*}\n",
    "\n",
    "which clearly demonstrates that the slack is essentially intentional error, where the optimiser can choose to move components of the desired end-effector motion into the slack vector. For both redundant and non-redundant robots, this means that the robot may stray from the straight line motion to improve manipulability and avoid a singularity, avoid running into joint position limits, or stay bounded by the joint velocity limits.\n",
    "\n",
    "Lets put this algorithm to the test in Python"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52016/?53016\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x2910cf990>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make a new environment and add objects\n",
    "env = Swift()\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "env.set_camera_pose([1.3, 0, 0.4], [0, 0, 0.3])\n",
    "env.add(panda)\n",
    "env.add(ee_axes)\n",
    "env.add(goal_axes) \n",
    "\n",
    "# Change the robot configuration to a ready position\n",
    "panda.q = [0.21, -0.03, 0.35, -1.90, -0.04, 1.96, 1.36]\n",
    "\n",
    "# A variable to specify when to break the loop\n",
    "arrived = False\n",
    "\n",
    "# Specify the gain for the p_servo method\n",
    "kt = 1.0\n",
    "kr = 1.3\n",
    "k = np.array([kt, kt, kt, kr, kr, kr])\n",
    "\n",
    "# Specify our timestep\n",
    "dt = 0.05\n",
    "\n",
    "Tep = (\n",
    "    panda.fkine(panda.q)\n",
    "    * sm.SE3.Tx(-0.1)\n",
    "    * sm.SE3.Ty(0.6)\n",
    "    * sm.SE3.Tz(0.4)\n",
    ")\n",
    "Tep = Tep.A\n",
    "\n",
    "# Set the goal axes to Tep\n",
    "goal_axes.T = Tep\n",
    "\n",
    "# Set the gain on the manipulability maximisation\n",
    "λm = 1.0\n",
    "\n",
    "# Set the gain on the joint velocity norm minimisation\n",
    "λq = 0.1\n",
    "\n",
    "# Run the simulation until the robot arrives at the goal\n",
    "while not arrived:\n",
    "\n",
    "    # Work out the base frame manipulator Jacobian using the current robot configuration\n",
    "    J = panda.jacob0(panda.q)\n",
    "\n",
    "    # Calculate the manipulability Jacobian\n",
    "    Jm = panda.jacobm(panda.q, axes='rot')\n",
    "\n",
    "    # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n",
    "    Te = panda.fkine(panda.q).A\n",
    "\n",
    "    # Spatial error\n",
    "    e = np.sum(np.abs(rtb.angle_axis(Te, Tep)))\n",
    "\n",
    "    # Calculate the required end-effector velocity and whether the robot has arrived\n",
    "    ev, arrived = rtb.p_servo(Te, Tep, gain=k, threshold=0.001, method=\"angle-axis\")\n",
    "\n",
    "    ### Calculate each component of the quadratic programme\n",
    "    # Quadratic component of objective function\n",
    "    Q = np.eye(panda.n + 6)\n",
    "\n",
    "    # Joint velocity component of Q\n",
    "    Q[:panda.n, :panda.n] *= λq\n",
    "\n",
    "    # Slack component of Q\n",
    "    Q[panda.n:, panda.n:] = (1 / e) * np.eye(6)\n",
    "\n",
    "    # The equality contraints\n",
    "    Aeq = np.c_[J, np.eye(6)]\n",
    "    beq = ev.reshape((6,))\n",
    "\n",
    "    # The inequality constraints for joint limit avoidance\n",
    "    Ain = np.zeros((panda.n + 6, panda.n + 6))\n",
    "    bin = np.zeros(panda.n + 6)\n",
    "\n",
    "    # The minimum angle (in radians) in which the joint is allowed to approach\n",
    "    # to its limit\n",
    "    ps = 0.05\n",
    "\n",
    "    # The influence angle (in radians) in which the velocity damper\n",
    "    # becomes active\n",
    "    pi = 0.9\n",
    "\n",
    "    # The gain for the velocity damper\n",
    "    k_qlim = 1.0\n",
    "\n",
    "    # Form the joint limit velocity damper\n",
    "    Ain[:panda.n, :panda.n], bin[:panda.n] = joint_velocity_damper(panda, ps, pi, k_qlim)\n",
    "\n",
    "    # Linear component of objective function: the manipulability Jacobian\n",
    "    c = np.r_[λm * -Jm.reshape((panda.n,)), np.zeros(6)]\n",
    "\n",
    "    # The lower and upper bounds on the joint velocity and slack variable\n",
    "    lb = np.r_[qd_lb, -10 * np.ones(6)]\n",
    "    ub = np.r_[qd_ub, 10 * np.ones(6)]\n",
    "\n",
    "    # Solve for the joint velocities qd and apply to the robot\n",
    "    xd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=lb, ub=ub, solver='quadprog')\n",
    "\n",
    "    # Apply the joint velocities to the Panda\n",
    "    panda.qd[:panda.n] = xd[:panda.n]\n",
    "\n",
    "    # Update the ee axes\n",
    "    ee_axes.T = Te\n",
    "\n",
    "    # Step the simulator by dt seconds\n",
    "    env.step(dt)"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "528815e074ebcdb9b34bcb695d4aa9d425bdb2cc6656b4ca45050b51a4125937"
  },
  "kernelspec": {
   "display_name": "Python 3.9.5 ('rtb')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.0"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
